#include <Stepper.h>
#include <Arduino.h>
//定义步进电机的步数和步进角
//14 Y-D+ 17 X-D+
//12 Y-P+ 5 X-P+
//X 60-3  26.5
//Y 50-2  UP 25.4 DOWN 24.9
//传送带 25

#include "em_config.h"
#include "hal/em_ble.h"
#include "hal/em_motor.h"
#include "hal/em_alg.h"
#include <Stepper.h>
#include <BluetoothSerial.h>

uint8_t a[]={54,90,126,58,90,122,68,90,112};
uint8_t b[]={52,51,52,67,54,67,68,68,68};
uint8_t c[]={109,112,109,117,126,117,128,112,128};
float x[]={0.0,19.0,42.0};
float y[]={0.0,15.0};
float now[]={0.0,0.0};
float temp[]={0.0,0.0};
int flag=0;
int o=9;
float move[8]={(26.5*3000)/57,(26.5*3700)/57,(26.5*3700)/57,0,0,0,0,(26.5*2200)/57};
BluetoothSerial mySerial;
int stateX=0;
int stateY=0;
uint16_t Arm_Mode[]={0,0,0,0,0,0}; //1-6号  药盒的药数

void motor_run_X()
{
  digitalWrite(5, 1);
  delayMicroseconds(500); // 延时 0.5 毫秒
  digitalWrite(5, 0);
  delayMicroseconds(500); // 延时 0.5 毫秒
}

void motor_run_Y()
{
  digitalWrite(12, 1);
  delayMicroseconds(500); // 延时 0.5 毫秒
  digitalWrite(12, 0);
  delayMicroseconds(500); // 延时 0.5 毫秒
}

void motor_run_X_2()
{
  digitalWrite(5, 1);
  delayMicroseconds(1000); // 延时 1 毫秒
  digitalWrite(5, 0);
  delayMicroseconds(1000); // 延时 1 毫秒
}

void motor_run_Y_2()
{
  digitalWrite(12, 1);
  delayMicroseconds(1000); // 延时 1 毫秒
  digitalWrite(12, 0);
  delayMicroseconds(1000); // 延时 1 毫秒
}

void setup()
{
  Serial.begin(9600);
  Serial.print("init_task\n");
  pinMode(12, OUTPUT);
  pinMode(14, OUTPUT);
  pinMode(25, OUTPUT);
  //pinMode(27, OUTPUT);
  //pinMode(26, OUTPUT);
  pinMode(17, OUTPUT);
  pinMode(5, OUTPUT);
  digitalWrite(25,0);
  digitalWrite(14,0);//d
  //digitalWrite(26,1);//d
  digitalWrite(17,0);//d x
  mySerial.begin(9600);   
  mySerial.begin("BNGU");
  //init_ble();
  em_motor_init();
  motor_run_X();
  delay(3000);
}

/*void loop() 
{
  unsigned long startTime=millis();
  while(millis()-startTime<=5000)
  {
    motor_run_Y();
    if(millis()-startTime<=3000)
    {
      motor_run_X();
    }
  }
  while(1)
 {

 }
}*/

void go()
{
  digitalWrite(17,0);//d x
}

void back()
{
  digitalWrite(17,1);//d x
}

void move_XY(int run_x_t,int run_y_t)
{
  digitalWrite(14, LOW);
  digitalWrite(17, LOW); 
  if(run_x_t>=0)
  {
    if(run_y_t>=0)
    {
      digitalWrite(14,0);//d
      digitalWrite(17,0);//d x
    }
    else 
    {
      digitalWrite(14,1);//d
      digitalWrite(17,0);//d x
    }
  }

  if(run_x_t<0)
  {
    if(run_y_t>=0)
    {
      digitalWrite(14,0);//d
      digitalWrite(17,1);//d x
    }
    else
    {
      digitalWrite(14,1);//d
      digitalWrite(17,1);//d x
    }
  }

{
  if (run_x_t == 0 && run_y_t == 0) 
  {
    return;
  }

  if (run_x_t == 0 && run_y_t != 0)
 {
    
    unsigned long startTime = millis();
    while (millis() - startTime <= fabs(run_y_t) ) 
    {
        motor_run_Y();
    }
    return;
}

// 只有 X 方向有步数
if (run_x_t != 0 && run_y_t == 0) 
{
    unsigned long startTime = millis();
    while (millis() - startTime <= fabs(run_x_t) )
    {
      motor_run_X();
    }
    return;
}

if (run_x_t != 0 && run_y_t != 0)
{
 // 两个方向都有步数
 if (abs(run_x_t) >= abs(run_y_t)) 
 {
   unsigned long startTime = millis();
   while (millis() - startTime <= fabs(run_x_t))
   {
     motor_run_X();      
   }
   startTime = millis();
   while (millis() - startTime <= fabs(run_y_t))
   {
     motor_run_Y();
   }
  } 
 else 
 {
   unsigned long startTime = millis();
   while (millis() - startTime <= fabs(run_y_t) )
   {
      motor_run_Y();
   }
   startTime = millis();
   while (millis() - startTime <= fabs(run_x_t))
   {
     motor_run_X();      
   }
   } 
  }
 }
}

void stepper_to_pos(float a,float b)
{
  float run_x_t=a*((26.5*1500)/57/4);
  float run_y_t=b*((25.15*1000)/48/4);
  now[0]+=a;
  now[1]+=b;
  a=0;
  b=0;
  move_XY(run_x_t,run_y_t);
  flag=1;
}

void armposition(uint16_t position,uint8_t Serial_Number)
{
  float run_x_t=0,run_y_t=0;
  if(position>0)
  {
    if(Serial_Number==0)
    {
      temp[0]=x[0]-now[0];
      temp[1]=y[0]-now[1];
      stepper_to_pos(temp[0],temp[1]);
    }

    if(Serial_Number==1)
    {
      temp[0]=x[1]-now[0];
      temp[1]=y[0]-now[1];
      stepper_to_pos(temp[0],temp[1]);
    }

    if(Serial_Number==2)
    {
      temp[0]=x[2]-now[0];
      temp[1]=y[0]-now[1];
      stepper_to_pos(temp[0],temp[1]);
    }

    if(Serial_Number==3)
    {
      temp[0]=x[2]-now[0];
      temp[1]=y[1]-now[1];
      stepper_to_pos(temp[0],temp[1]);
    }

    if(Serial_Number==4)
    {
      temp[0]=x[1]-now[0];
      temp[1]=y[1]-now[1];
      stepper_to_pos(temp[0],temp[1]);
    }

    if(Serial_Number==5)
    {
      temp[0]=x[0]-now[0];
      temp[1]=y[1]-now[1];
      stepper_to_pos(temp[0],temp[1]);
    }

  }
  else
  {
    return;
  }
}

void armpose(uint8_t armmode,uint8_t i)
{
  if(i==0)
  {
  if(armmode==1)
  {
    em_motor_run_by_angle(180,0,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,180);
    delay(750);
    em_motor_run_by_angle(180,0,180,180);
    delay(750);
    em_motor_run_by_angle(0,0,180,180);
    delay(1500);
    em_motor_run_by_angle(0,53,147,90);
    delay(1500);  
    em_motor_run_by_angle(180,0,180,90);
    delay(1000);
  }

  if(armmode==2)
  {
    go();
    unsigned long startTime = millis();
    while (millis() - startTime <= fabs(move[0]) )
    {
        motor_run_X_2();
    }
    em_motor_run_by_angle(180,0,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,180);
    delay(750);
    em_motor_run_by_angle(180,0,180,180);
    delay(750);
    em_motor_run_by_angle(0,0,180,180);
    delay(1500);
    em_motor_run_by_angle(0,53,147,90);
    delay(1500);  
    em_motor_run_by_angle(180,0,180,90);
    delay(1000);
    back();
    unsigned long startTime2 = millis();
    while (millis() - startTime2 <= fabs(move[0]) )
    {
      motor_run_X_2();
    }
    
  }
  }

  else if(i==1)
  {
    if(armmode==1)
  {
    em_motor_run_by_angle(180,0,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,180);
    delay(750);
    em_motor_run_by_angle(180,44,180,180);
    delay(500);
    em_motor_run_by_angle(180,0,180,180);
    delay(750);
    em_motor_run_by_angle(0,0,180,180);
    delay(1500);
    back();
    unsigned long startTime = millis();
    while (millis() - startTime <= fabs(move[7]) )
    {
      motor_run_X();
    }
    em_motor_run_by_angle(0,53,147,90);
    delay(1500);  
    em_motor_run_by_angle(180,0,180,90);
    delay(1000);
    now[0]=now[0]-((move[7])/((26.5*1500)/57/4));
  }

  if(armmode==2)
  {
    armposition(1,1);
    em_motor_run_by_angle(180,0,180,90);
    delay(500);
    em_motor_run_by_angle(180,52,180,90);
    delay(500);
    em_motor_run_by_angle(180,44,180,180);
    delay(750);
    em_motor_run_by_angle(180,0,180,180);
    delay(750);
    em_motor_run_by_angle(0,0,180,180);
    delay(1500);
    back();
    unsigned long startTime2 = millis();
    while (millis() - startTime2 <= fabs(move[7]) )
    {
      motor_run_X();
    }
    em_motor_run_by_angle(0,53,147,90);
    delay(1500);  
    em_motor_run_by_angle(180,0,180,90);
    //delay(1000);
    now[0]=now[0]-((move[7])/((26.5*1500)/57/4));
  }
}

else if(i==5)
  {
    if(armmode==1)
  {
    digitalWrite(14,0);//d
    unsigned long startTime = millis();
    while (millis() - startTime <= fabs(move[1]) )
    {
      motor_run_Y_2();
    }
    em_motor_run_by_angle(180,0,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,180);
    delay(750);
    em_motor_run_by_angle(180,0,180,180);
    delay(750);
    em_motor_run_by_angle(0,0,180,180);
    delay(1500);
    digitalWrite(14,1);//d
    unsigned long startTime2 = millis();
    while (millis() - startTime2 <= fabs(move[2]) )
    {
        motor_run_Y_2();
    }
    em_motor_run_by_angle(0,53,147,90);
    delay(1500);  
    em_motor_run_by_angle(180,0,180,90);
    delay(1000);
    digitalWrite(14,0);//d
   /*em_motor_run_by_angle(a[0],b[0],c[0],90);
    delay(1000);
    em_motor_run_by_angle(a[0],b[0],c[0],180);
    delay(1000);
    em_motor_run_by_angle(90,0,180,180);
    delay(1000);
    em_motor_run_by_angle(90,0,180,90);
    delay(500);*/
  }

  if(armmode==2)
  {
    em_motor_run_by_angle(180,0,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,90);
    delay(500);
    em_motor_run_by_angle(180,48,180,180);
    delay(750);
    em_motor_run_by_angle(180,0,180,180);
    delay(750);
    em_motor_run_by_angle(0,0,180,180);
    delay(1500);
    em_motor_run_by_angle(0,53,147,90);
    delay(1500);   
    em_motor_run_by_angle(180,0,180,90);
    delay(1000);
    back();
  }
}
}

void loop()
{
  digitalWrite(25,0); 
  uint16_t Medicine_Box_Callback[]={0,0,0,0,0,0}; //1-6号 药柜
  
/*
  while(1)
  {
    if(digitalRead(PIN_KEY1)==LOW)
    {
        delay(20);
        if(digitalRead(PIN_KEY1)==LOW)
        {
          delay(20);
          Medicine_Box_Callback[0]+=1;
          if(Medicine_Box_Callback[0]>=9)
          {
            Medicine_Box_Callback[0]=9;
          }
          delay(500);
        }
    }

    if(digitalRead(PIN_KEY2)==LOW)
    {    
      delay(20);
      if(digitalRead(PIN_KEY2)==LOW)
      { 
        delay(20);
        Medicine_Box_Callback[1]+=1;
        if(Medicine_Box_Callback[1]>=9)
        {
          Medicine_Box_Callback[1]=9;
        }
        delay(500);
      }
    }

    if(digitalRead(PIN_KEY3)==LOW)
    {
        break;
    }
  }
*/

while (1)
{  
    if(mySerial.available()>0)
    {
      o=mySerial.read();
      mySerial.write(o);
    }

    switch (o)
    {
    case '1':
        Medicine_Box_Callback[0]++;
        if(Medicine_Box_Callback[0]>2)
        {
          Medicine_Box_Callback[0]=2;
          mySerial.print("No_Medicine\n");
        }
        o=9;
        break;
    case '2':
        Medicine_Box_Callback[1]++;
        if(Medicine_Box_Callback[1]>2)
        {
          Medicine_Box_Callback[1]=2;
          mySerial.print("No_Medicine\n");
        }
        o=9;
        break;
    case '3':
        Medicine_Box_Callback[2]++;
        if(Medicine_Box_Callback[2]>2)
        {
          Medicine_Box_Callback[2]=2;
          mySerial.print("No_Medicine\n");
        }
        o=9;
        break;
    case '4':
        Medicine_Box_Callback[3]++;
        if(Medicine_Box_Callback[3]>2)
        {
          Medicine_Box_Callback[3]=2;
          mySerial.print("No_Medicine\n");
        }
        o=9;
        break;
    case '5':
        Medicine_Box_Callback[4]++;
        if(Medicine_Box_Callback[4]>2)
        {
          Medicine_Box_Callback[4]=2;
          mySerial.print("No_Medicine\n");
        }
        o=9;
        break;
    case '6':
        Medicine_Box_Callback[5]++;
        if(Medicine_Box_Callback[5]>2)
        {
          Medicine_Box_Callback[5]=2;
          mySerial.print("No_Medicine\n");
        }
        o=9;
        break;
    case '0':
      for(int i=0;i<6;i++)
      {
         mySerial.printf("%d",Medicine_Box_Callback[i]);
      }
        break;
    default:
	      break;
    }

    if(o=='0')
    {
      o=9;
      break;
    }
}

  for(uint8_t i=0;i<6;i++)
  {
    armposition(Medicine_Box_Callback[i],i);
    delay(50);
    
    if(flag==1)
    {    
      for(Medicine_Box_Callback[i];Medicine_Box_Callback[i]>0;Medicine_Box_Callback[i]--)
      {
        if(Arm_Mode[i]==2)
        {
          return;
        }
        Arm_Mode[i]++;
        armpose(Arm_Mode[i],i);
        delay(10);
      }      
      flag=0;
    } 
    //armposition(1,0);
  }
  digitalWrite(25,1);
  armposition(1,0);
  delay(8000); 
}
